package com.rockwell.robot2015.model;

import java.util.HashSet;

import com.rockwell.robot2015.model.command.RobotCommand;
import com.rockwell.robot2015.model.detectors.IDetector;
import com.rockwell.robot2015.model.sensors.IRobotSensors;
import com.rockwell.robot2015.model.solvers.IMazeSolver;
import com.rockwell.robot2015.model.states.RobotState;
import com.rockwell.robot2015.model.states.RobotStateMachine;
import com.rockwell.robot2015.model.states.running.RobotRunningState;
import com.rockwell.robot2015.model.states.running.RobotRunningStateMachine;
import com.rockwell.robot2015.model.states.running.sequence.RobotRunningSequence;

import android.annotation.SuppressLint;
import android.util.Log;


@SuppressLint("DefaultLocale")
public class Robot {
	static final String TAG = "robot";
	
	public static int INTERVAL = 50;
	public static int CMD_TIME_OUT = 15000; // Milliseconds
	public static int PICK_TIME_OUT = 120000; // Milliseconds
	public static int MOTION_SETTLING_TIME = 2000; // Milliseconds
	public static int ALIGNMENT_THRESHOLD = 5;
	public static int LANE_WIDTH = 300; // mm
	public static int FRONT_SENSOR_THRESHOLD = 100; // mm
	public static int DETECT_FRONT_WALL_INDEX = 20; // mm
	public static int LEFT_MOVEBACKWARD_WITHOUT_FRONT_WALL = 50; // mm
	public static int RIGHT_MOVEBACKWARD_WITHOUT_FRONT_WALL = 30; // mm
	public static int MOVE_INDEX = 30; // mm
	public static int UTURN_MOVE_INDEX = 30; //mm
	public static int TURN_INDEX = 3; // Degree
	public static int SMALL_TURN_INDEX = 2; // Degree
	public static int SMALL_MOVE_INDEX = 15; // mm
	public static int SIDE_SENSOR_OFFSET = 40; // mm
	public static int DETECT_FRONT_WALL_DISTANCE = 400; // mm
	public static int LEFT_REVERSE_INTO_JUNCTION = 100; // mm
	public static int MAX_PICK_TREASURE_RETRY = 1;
	public static int RIGHT_REVERSE_INTO_JUNCTION = 80; // mm
	public static int ADJUST_ANGLE = 0; // Degree
	public static int COMPENSATION_ANGLE_TOLERANCE = 3; // Degree
	public static int ADJUST_ANGLE_THRESHOLD = 10; // Degree
	
	enum CommandStatus {
		Done,
		Busy,
		Timeout,
		Error
	}
	
	private int m_cmdTimeOut = 0;
	private RobotPosition m_robotPosition;
	private int m_idealOrientation = 0;
	private int m_logLevel = 1;
	private boolean m_stepMode = false;
	private boolean m_useCompassCompensation = false;

	private double m_robotWidth = 15; // 15cm
	private double m_robotLength = 15; // 15cm
	
	private boolean m_gameOver = false;
	
	private Runnable m_mission = new Runnable() {

		@Override
		public void run() {
			log("Robot Started", 1);
			while(true) {
				try {
					continueMission();
				}
				catch (Exception ex) {
					log(ex, 1);
					//pause();
				}
			}
		}
		
	};

	private Thread m_backgroundThread = new Thread(m_mission);
	
	private RobotConfig m_config;
	
	private RobotStateMachine m_stateMachine;
	
	// avoid creating lots of RobotCommand objects
	private RobotCommand m_command = new RobotCommand();
	
	private IRobotLogger m_logger;
	
	private boolean m_stopped = false;
	
	private GameStage m_gameStage;
	
	private int m_treasurePicked;
	
	MazeType m_currentMaze;

	private HashSet<MazeType> m_mazeVisited = new HashSet<MazeType>();
	
	public Robot(RobotConfig config, GameStage gameStage) {
		m_config = config;
		m_gameStage = gameStage;
		
		m_stateMachine = new RobotStateMachine(this);
	}

	public void setLogger(IRobotLogger logger, int level) {
		m_logger = logger;
		m_logLevel = level;
	}
	
	public void log(String msg, int level) {
		if (m_logger != null && m_logLevel >= level) {
			m_logger.log(msg);
			
			if ((m_stepMode && level <= 2) || level == 0) {
				/*
				runBeforePause();
				waitForResume();
				runBeforeStart();
				*/
			}
		}
	}
	
	public void log(Exception ex, int level) {
		if (m_logger != null && m_logLevel >= level) {
			m_logger.log(ex);
		}
	}

	public void setCurrentState(String state) {
		m_logger.setCurrentState(state);
	}
	
	public void onRobotPaused() {
		m_logger.onRobotPaused();
	}
	
	public void onRobotResumed() {
		m_logger.onRobotResumed();
	}
	
	public void onRobotIdle() {
		m_logger.onRobotIdle();
	}

	public double getRobotWidth() {
		return m_robotWidth;
	}
	
	public void setRobotWidth(double width) {
		m_robotWidth = width;
	}
	
	public double getRobotLength() {
		return m_robotLength;
	}
	
	public void setRobotLength(double length) {
		m_robotLength = length;
	}
	
	public void setOrientation(float angle) {
		m_config.sensors.setOrientation(angle);
	}
	
	public void setStepMode(boolean step) {
		m_stepMode = step;
	}
	
	public boolean isInStepMode() {
		return m_stepMode;
	}
	
	public IMazeSolver getPredefinedMazeSolver() {
		return m_config.predefinedMazeSolver;
	}
	
	/**
	 * The location of the sensor from the left edge of the robot.
	 * @return
	 */
	public double getLeftDistanceSensorLocation() {
		return 2.5;	
	}
	
	/**
	 * The location of the sensor from the front of the robot.
	 * @return
	 */
	public double getFronDistanceSensorLocation() {
		return 1.5;
	}
	
	public void setUseCompassCompensation(boolean use) {
		m_useCompassCompensation = use;
	}
	
	public IDetector getTreasureDetector() {
		return this.m_config.treasureDetector;
	}
	
	public IDetector getDropAreaDetector() {
		return this.m_config.dropAreaDetector;
	}
	
	public IDetector getMaze1Detector() {
		//using this to detect maze wall
		return this.m_config.maze1Detector;
	}
	
	public IDetector getMaze2Detector() {
		//using this to detect maze wall
		return this.m_config.maze2Detector;
	}
	
	public IDetector getMaze3Detector() {
		//using this to detect maze wall
		return this.m_config.maze3Detector;
	}
	
	public IDetector getMazeEntranceGuideDetector() {
		return m_config.mazeEntranceGuideDetector;
	}
	
	/*
	public IDetector getObstacleDetector() {
		return this.m_config.obstacleDetector;
	}
	*/
	
	public IDetector getOuterWallDetector() {
		return this.m_config.outerWallDetector;
	}

	public void enableTreasureDetector(boolean enabled) {
		m_config.treasureDetector.setEnabled(enabled);
	}
	
	public void enableDropAreaDetector(boolean enabled) {
		m_config.dropAreaDetector.setEnabled(enabled);
	}
	
	public void enableMaze1Detector(boolean enabled) {
		//using this to detect maze wall
		m_config.maze1Detector.setEnabled(enabled);
	}
	
	public void enableMaze2Detector(boolean enabled) {
		//using this to detect maze wall
		m_config.maze2Detector.setEnabled(enabled);
	}
	
	public void enableMaze3Detector(boolean enabled) {
		//using this to detect maze wall
		m_config.maze3Detector.setEnabled(enabled);
	}
	
	public void enableMazeEntranceGuideDetector(boolean enabled) {
		m_config.mazeEntranceGuideDetector.setEnabled(enabled);
	}
	
	/*
	public void enableObstacleDetector(boolean enabled) {
		m_config.obstacleDetector.setEnabled(enabled);
	}
	*/
	public void enableOuterWallDetector(boolean enabled) {
		m_config.outerWallDetector.setEnabled(enabled);
	}

	public IRobotSensors getSensor() {
		return this.m_config.sensors;
	}
	
	public RobotStateMachine stateMachine() {
		return m_stateMachine;
	}
	
	public boolean shouldDetectObstacle() {
		return m_gameStage == GameStage.Finals;
	}
	
	public boolean isTieBreaker() {
		return m_gameStage == GameStage.TieBreaker;
	}
	
	public void start(boolean startThread) throws Exception {
		m_gameOver = false;
		m_stateMachine.setNextState(RobotState.Initial);
		m_stopped = false;
		
		m_config.commander.connect();
		
		if (m_config.commander.isConnected()) {
			
			if (m_logger != null)
				m_logger.log("Modbus connected");
			
			//m_missionHandler.postDelayed(m_mission, INTERVAL);
			if(startThread) {
				m_backgroundThread.start();
				
				if (m_logger != null)
					m_logger.log("thread started");
			}
			
			Log.i(TAG, "started");
		}
		else {
			throw new Exception("Cannot establish Modbus connection.");
		}
	}

	public void quit() {
		m_gameOver = true;
		
		Log.i(TAG, "quit");
	}
	
	public void initialize() {
		sleep(2000);
		
		reconnectModbus();
		m_config.commander.reset();
		m_idealOrientation = (int)m_config.sensors.getOrientation();
		
		// make sure the arm is up
		//if (!getSensor().isArmUp()) {
		//	sendCommand(RobotAction.ArmUp);
		//}
		
		String msg = String.format("Robot initialized");
		log(msg, 2);
	}
	
	public void restart() {
		log("Robot starting", 2);
		
		reconnectModbus();
		
		m_stateMachine.reset();
		m_stateMachine.start();
		
		if (isInStepMode()) {
			log("Robot started in Step mode", 2);
		}
		else {
			log("Robot started in Run mode", 2);
		}
	}
	
	public boolean isStopped() {
		return this.m_stateMachine.getState().equals(RobotState.Stopped);
	}
	
	public void stop() {
		m_stopped = true;
		
		m_stateMachine.stop();
		
		log("Robot stopped", 2);
	}
	
	public void ensureArmAtHome() {
		boolean armIsUp = m_config.sensors.isArmUp();
		
		log("Arm is up:" + armIsUp, 4);
		
		if (!armIsUp) {
			sendCommand(RobotAction.ArmUp);
		}
	}
	
	public boolean isPaused() {
		return (m_stateMachine.isPaused());
	}
	
	public void resume() {
		m_stateMachine.resume();
		log("Robot resumed", 2);
	}
	
	public void pause() {
		m_stateMachine.pause();
		log("Robot paused", 2);
	}
	
	public void reset() {
		m_treasurePicked = 0;
		
		m_stateMachine.reset();
	}
	
	public void setNewRunningState(RobotRunningState newState) {
		m_stateMachine.setNewRunningState(newState);
	}
	
	public void onTreasurePicked() {
		m_treasurePicked++;
	}
	
	public void setCurrentMaze(MazeType mazeType) {
		m_currentMaze = mazeType;
	}
	
	public MazeType getCurrentMaze() {
		return m_currentMaze;
	}
	
	public void onTreasureDropped() {
		m_mazeVisited.add(m_currentMaze);
	}
	
	public boolean shouldTryMaze3() {
		return false;

		/*
		if (m_mazeVisited.size() > 1) {
			if (!m_mazeVisited.contains(MazeType.Maze1))
				return false;
			
			if (!m_mazeVisited.contains(MazeType.Maze2))
				return false;
			
			return true;
		}
		else {
			return false;
		}
		*/
	}
	
	public int treasurePicked() {
		return m_treasurePicked;
	}
	
	public void continueMission() {
		m_stateMachine.move();
		//sleep(20);
	}
	
	public boolean blocked() {
		return false;
	}
	
	private void reconnectModbus() {
		try {
			m_config.commander.disconnect();
			m_config.commander.connect();
		} catch (Exception e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
	}

	public boolean sendCommand(RobotAction action) {
		return sendCommand(action, 0) == CommandStatus.Done;
	}
	
	public boolean startJogging() {
		try {
			m_cmdTimeOut = 0;
			m_command.param1 = 0;
			m_command.action = RobotAction.Jog;
			
			m_config.commander.sendCommand(m_command);
			return true;
		}
		catch (Exception ex) {
			return false;
		}
	}
	
	public boolean startJogging2() {
		try {
			m_cmdTimeOut = 0;
			m_command.param1 = 0;
			m_command.action = RobotAction.Jog2;
			
			m_config.commander.sendCommand(m_command);
			return true;
		}
		catch (Exception ex) {
			return false;
		}
	}
	
	public boolean startJogging3() {
		try {
			m_cmdTimeOut = 0;
			m_command.param1 = 0;
			m_command.action = RobotAction.Jog3;
			
			m_config.commander.sendCommand(m_command);
			return true;
		}
		catch (Exception ex) {
			return false;
		}
	}
	
	private static final int MIN_ADJUST_ANGLE = 10;
	
	public boolean adjustAngle(int threshold) {
		boolean canAdjust = false;
		
		IRobotSensors sensor = getSensor();
		IRobotSensors.LeftAngle angle = sensor.getLeftAngle();
		
		int a = angle.angle;
		int tries = a / Math.min(MIN_ADJUST_ANGLE, threshold);
		
		for (int i = 0; i < tries; i++) {
			angle = sensor.getLeftAngle();
	
			if (!angle.leftFrontOpen && !angle.leftBackOpen){
				canAdjust = true;
				
				a = angle.angle;
				
				log(String.format("Angle to adjust: %d degrees, direction: %s", 
						a, angle.direction ? "left" : "right"), 4);

				if (a >= threshold) {
					
					if (Math.abs(a) > MIN_ADJUST_ANGLE) {
						a = MIN_ADJUST_ANGLE;
					}						

					a = angle.direction ? a : -a;
					
					log(String.format("Adjust angle: %d degrees", a), 4);

					sendCommand(RobotAction.TurnDegree, a);
					sleep(Robot.MOTION_SETTLING_TIME);
				}
				else {
					break;
				}
			}
			else {
				canAdjust = false;
				break;
			}
		}
		
		return canAdjust;
	}
	
	private int TURN_STUCK_THRESHOLD = 5;
	private int MOVE_BACK_DISTANCE = 5;
	
	public boolean turn(int degrees) {
		CommandStatus status = sendCommand(RobotAction.TurnDegree, degrees);
		
		if (status == CommandStatus.Timeout) {
			
			log("Turn Command timeout", 4);
			
			IRobotSensors sensor = m_config.sensors;
			
			int target = sensor.getLeftTargetValue();
			int present = sensor.getLeftPresentValue();
			
			int remaining = (int)(degrees * (1 - (double)present / target));
			
			if (remaining >= TURN_STUCK_THRESHOLD) {
				sendCommand(RobotAction.MoveWithSensor, -MOVE_BACK_DISTANCE);
				sleep(MOTION_SETTLING_TIME);
				
				sendCommand(RobotAction.TurnDegree, remaining);
				sleep(MOTION_SETTLING_TIME);
			}
			
			return true;
		}
		else {
			return status == CommandStatus.Done;
		}
	}
	
	public boolean turnLeft() {
		
		return turn(90);
	}
	
	public boolean turnRight() {
		return turn(-90);
	}
	
	public void turnBack() {
		IRobotSensors sensor = getSensor();
		
		double leftDistance = sensor.getLeftDistance();
		double rightDistance = sensor.getRightDistance();
		
		if (leftDistance > rightDistance) {
			turn(180);
		}
		else {
			turn(-180);
		}
	}
	
	static final int MAX_TRIES = 3;
	
	public CommandStatus sendCommand(RobotAction robotAction, int param) {
/*
		if (robotAction != RobotAction.ArmUp && robotAction != RobotAction.ArmDown) {
			ensureArmAtHome();
		}
*/
		
		CommandStatus status = CommandStatus.Done;
		
		m_cmdTimeOut = 0;
		m_command.param1 = param;
		m_command.action = robotAction;

		boolean done = false;
		boolean send = true;
		boolean timeout = false;
		
		int trial = 0;
		
		String msg;
		
		do {
			if (send) {
				//String msg = String.format("Sending Command %s...", robotAction.name());
				//log(msg, 4);

				try {
					trial++;
					m_config.commander.sendCommand(m_command);
				}
				catch (Exception ex) {
					log("Failed to send command: " + ex.getMessage(), 2);
					
					sleep(INTERVAL);
					reconnectModbus();
					sleep(INTERVAL);
					
					if (trial <= MAX_TRIES) {
						continue;
					}
					else {
						log("Failed to resend command: " + ex.getMessage(), 2);
						
						return CommandStatus.Error;
					}
				}
			}
			
			// Command has been sent successfully, wait until the command is done.
			
			status = peekCommandDone(CMD_TIME_OUT);
			
			switch (status) {
			case Done:
				msg = String.format("Command %s(%d) sent successfully.", robotAction.name(), param);
				log(msg, 4);
				
				done = true;
				break;
				
			case Busy:
				send = false;
				break;
			
			case Timeout: 
				if (timeout = false) {
					timeout = true;
					msg = String.format("Command time out", robotAction.name());
					log(msg, 0);
				}

				break;
				
			case Error:
				//msg = String.format("Command error", robotAction.name());
				//log(msg, 0);

				reconnectModbus();
				sleep(INTERVAL);
				
				// resend the command
				send = true;
				
				break;
			}
		} while (!done && !timeout);
		
		return status;
	}
	
	public CommandStatus peekCommandDone(int timout) {
		try {
			if (m_config.commander.isCommandDone()) {
				return CommandStatus.Done;
			}
			else if (m_cmdTimeOut < timout){
				m_cmdTimeOut += INTERVAL;
				sleep(INTERVAL);
				return CommandStatus.Busy;
			}
			else {
				return CommandStatus.Timeout;
			}
		}
		catch (Exception ex) {
			//m_gameOver = true;
			log(ex, 1);
			
			return CommandStatus.Error;
		}
	}
	
	public boolean isJogging() {
		return m_config.commander.isJogging();
	}
	
	public boolean isJogging2() {
		return m_config.commander.isJogging();
	}
	
	public boolean isJogging3() {
		return m_config.commander.isJogging3();
	}
	
	public boolean isJogDone() {
		try {
			return m_config.commander.isJogDone();
		}
		catch(Exception ex) {
			return true;
		}
	}
	
	public void sleep(int duration) {
		try {
			Thread.sleep(duration);
		} catch (InterruptedException ex) {
			log(ex, 1);
		}
	}
	
	public interface ISequenceTester {
		boolean shouldStop();
	}
	
	private boolean testDone(RobotRunningSequence sequence, ISequenceTester tester) {
		return tester.shouldStop() || sequence.succeeded() || sequence.failed();
	}
	
	private void testSequence(RobotRunningSequence sequence, ISequenceTester tester) {
		m_stateMachine.reset();
		
		reconnectModbus();

		do {
			sequence.move();
			sleep(20);
		} while(!testDone(sequence, tester));
		
		sendCommand(RobotAction.Stop);
	}
	
	public void testFindTreasure(ISequenceTester tester) {
		RobotRunningStateMachine running = m_stateMachine.runningStateMachine();
		testSequence(running.findingTreasureSequence(), tester);
	}
	
	public void testApproachTreasure(ISequenceTester tester) {
		RobotRunningStateMachine running = m_stateMachine.runningStateMachine();
		testSequence(running.approachingTreasureSequence(), tester);
	}

	public void testPickTreasure(ISequenceTester tester) {
		RobotRunningStateMachine running = m_stateMachine.runningStateMachine();
		testSequence(running.pickingTreasureSequence(), tester);
	}

	public void testFindMaze(ISequenceTester tester) {
		RobotRunningStateMachine running = m_stateMachine.runningStateMachine();
		testSequence(running.findingMazeSequence(), tester);
	}
	
	public void testApproachMaze(ISequenceTester tester) {
		RobotRunningStateMachine running = m_stateMachine.runningStateMachine();
		testSequence(running.approachingMazeSequence(), tester);
	}

	public void testFindMazeEntrance(ISequenceTester tester) {
		RobotRunningStateMachine running = m_stateMachine.runningStateMachine();
		testSequence(running.findingMazeEntranceSequence(), tester);
	}

	public void testEnterMaze(ISequenceTester tester) {
		RobotRunningStateMachine running = m_stateMachine.runningStateMachine();
		testSequence(running.enteringMazeSequence(), tester);
	}
	
	public void testFindDropArea(ISequenceTester tester) {
		RobotRunningStateMachine running = m_stateMachine.runningStateMachine();
		testSequence(running.findingDropAreaSequence(), tester);
	}
	
	public void testDropTreasure(ISequenceTester tester) {
		RobotRunningStateMachine running = m_stateMachine.runningStateMachine();
		testSequence(running.droppingTreasureSequence(), tester);
	}
	
	public void testMaze3(ISequenceTester tester) {
		RobotRunningStateMachine running = m_stateMachine.runningStateMachine();
		setCurrentMaze(MazeType.Maze3);
		testSequence(running.findingDropAreaSequence(), tester);
	}
	
	public void testExitMaze(ISequenceTester tester) {
		RobotRunningStateMachine running = m_stateMachine.runningStateMachine();
		testSequence(running.exitingMazeSequence(), tester);
	}
}
